#include "servicephysics.h"

#include "planetcell.h"
#include "worldgridmanager.h"

namespace infinity {

/*static*/ const ServiceKey ServicePhysics::Key = ServiceKey("Service::Physics");
/*static*/ const dw::float32 ServicePhysics::MaxSpeed = 10.0f; // 10.0 m.s-1 =36 km.h-1

/*virtual*/ ServicePhysics::~ServicePhysics()
{
	dwAssert(m_objects.empty(), "Object collection is not empty");
}

physics::Object* ServicePhysics::createObject()
{
	physics::Object* newObject = new physics::Object();

	m_objects.push_back(newObject);

	newObject->m_shape.type = physics::Shape::Type_Sphere;
	newObject->m_shape.sphere.radius = 0.45f;

	return newObject;
}

void ServicePhysics::releaseObject(physics::Object* object)
{
	dwAssert(object != NULL, "Invalid argument : object is NULL");
	dwAssert(std::find(m_objects.begin(), m_objects.end(), object) != m_objects.end(), "Invalid argument : object is not a registered object");

	m_objects.erase(std::find(m_objects.begin(), m_objects.end(), object));

	delete object;
}

/*virtual*/ void ServicePhysics::onUpdateSimulation(dw::float32 deltaT)
{
	Planet::Cell cell;
	Vector3f objectPos;
	Vector3f objectVel;
	Vector3i objectCellId;

	for (ObjectsCollection::iterator objIt = m_objects.begin(); objIt != m_objects.end(); ++objIt)
	{
		physics::Object* object = *objIt;

		dwAssert(object != NULL, "Invalid internal state");

		objectPos = Vector3f(object->m_transform.pos);
		objectVel = object->m_velocity;

		//! velocity integration
		//! @{
		objectVel += deltaT * Vector3f(0.0f, -9.81f, 0.0f);

		dw::float32 speed = objectVel.length();
		if (speed > MaxSpeed)
		{
			dw::float32 ratio = MaxSpeed / speed;

			objectVel = ratio * objectVel;
		}
		//! @}

		//! collision detection
		//! @{
		dwAssert(object->m_shape.type == physics::Shape::Type_Sphere, "Invalid shape type");
		dwAssert(object->m_shape.sphere.radius <= 1.0f, "Invalid sphere radius");

		WorldGridManager& worldGridManager = WorldGridManager::getInstance();

		//! @todo cache cells
		for (int k = 0; k < 3; ++k)
		{
			int z = std::floor(objectPos.z) + k - 1;
			for (int j = 0; j < 3; ++j)
			{
				int y = std::floor(objectPos.y) + j - 1;
				for (int i = 0; i < 3; ++i)
				{
					int x = std::floor(objectPos.x) + i - 1;
					worldGridManager.getCell(x, y, z, cell);

					const Material& material = worldGridManager.getPlanet()->getMaterialLibrary().getMaterialFromId(cell.getMaterialId());

					if (!material.isVoid())
					{
						Vector3f cellMin(x, y, z);
						Vector3f cellMax(x + 1, y + 1, z + 1);

						Vector3f closestPoint;
						Vector3f closestPointFromSphere;

						closestPoint.x = std::max(std::min(objectPos.x, cellMax.x), cellMin.x);
						closestPoint.y = std::max(std::min(objectPos.y, cellMax.y), cellMin.y);
						closestPoint.z = std::max(std::min(objectPos.z, cellMax.z), cellMin.z);

						//! @todo work in world coordinates
						closestPointFromSphere = closestPoint - objectPos;
						if (closestPointFromSphere.length() < object->m_shape.sphere.radius) // contact
						{
							dw::float32 contactDepth = object->m_shape.sphere.radius - closestPointFromSphere.length();
							Vector3f contactNormal = closestPointFromSphere;
							contactNormal.normalize();

							dw::float32 velocityAlongNormal = Vector3f::dot(contactNormal, objectVel);

							if (velocityAlongNormal > 0.0f)
							{
								objectVel = objectVel - velocityAlongNormal * contactNormal;
								velocityAlongNormal = Vector3f::dot(contactNormal, objectVel);
							}

							// Drag
							Vector3f objectVelInPlane = objectVel - velocityAlongNormal * contactNormal;
							dw::float32 vel = objectVelInPlane.length();

							if (vel > std::numeric_limits< dw::float32 >::epsilon() &&
								std::abs(contactNormal.y) > 1.0f - std::numeric_limits< dw::float32 >::epsilon())
							{
								static const dw::float32 linearDragCoef = 0.05f;
								static const dw::float32 turbulentDragCoef = 0.02f;
								
								dw::float32 linearDrag = linearDragCoef * vel;
								dw::float32 turbulentDrag = 0.5f * turbulentDragCoef * 1.2f * 
									(dw::maths::pi * object->m_shape.sphere.radius * object->m_shape.sphere.radius) *
									vel * vel;
								dw::float32 totalDrag = linearDrag + turbulentDrag * deltaT;

								totalDrag = std::min(totalDrag, vel);

								Vector3f normalizedVelocity(objectVelInPlane);

								normalizedVelocity.normalize();

								objectVel += -totalDrag * normalizedVelocity;
							}

							objectPos = objectPos - contactDepth * contactNormal;
						}
					}
				}
			}
		}
		//! @}

		// Air drag
		objectCellId.x = std::floor(objectPos.x);
		objectCellId.y = std::floor(objectPos.y);
		objectCellId.z = std::floor(objectPos.z);

		worldGridManager.getCell(objectCellId.x, objectCellId.y, objectCellId.z, cell);

		const Material& objectCellMaterial = worldGridManager.getPlanet()->getMaterialLibrary().getMaterialFromId(cell.getMaterialId());
		if (objectCellMaterial.isVoid())
		{
			dw::float32 vel = objectVel.length();

			if (vel > std::numeric_limits< dw::float32 >::epsilon())
			{
				static const dw::float32 linearDragCoef = 0.001f;
				static const dw::float32 turbulentDragCoef = 0.001f;
				
				dw::float32 linearDrag = linearDragCoef * vel;
				dw::float32 turbulentDrag = 0.5f * turbulentDragCoef * 1.2f * 
					(dw::maths::pi * object->m_shape.sphere.radius * object->m_shape.sphere.radius) *
					vel * vel;
				dw::float32 totalDrag = linearDrag + turbulentDrag * deltaT;

				totalDrag = std::min(totalDrag, vel);

				Vector3f normalizedVelocity(objectVel);

				normalizedVelocity.normalize();

				objectVel += -totalDrag * normalizedVelocity;
			}
			else
			{
				objectVel = Vector3f::zero;
			}
		}

		objectPos += deltaT * objectVel;

		object->m_velocity = objectVel;
		object->m_transform.pos = dw::maths::Vector4f(objectPos, 1.0f);
	}
}

} // namespace infinity
